#include <Servo.h>

int ENA = 3;
int ENB = 6;
int INA = 12;
int INB = 13;
int INC = 14;
int IND = 15;
int trigPin = 7;
int echoPin = 8;

Servo servo1;
Servo servo2;

String inputString = "";
String sp;
boolean stringComplete = false;

void setup() 
{                
  // initialize the digital pin as an output.
  servo1.attach(10);
  servo2.attach(11);
  pinMode(INA, OUTPUT);  
  pinMode(INB, OUTPUT);
  pinMode(INC, OUTPUT);
  pinMode(IND, OUTPUT); 
  pinMode(ENA, OUTPUT); 
  pinMode(ENB, OUTPUT); 
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
  inputString.reserve(255);  // reserve 255 bytes for the inputString:
  while (!Serial);
  //setServo(servo1,90);
 // setServo(servo2,90);
  Serial.println("End");
  
}

void loop() 
{
  if (stringComplete) 
  {
    Serial.println(inputString); 
    if(inputString.indexOf("Se1")==0)
    {
      Serial.println("Servo 1");
      sp = inputString.substring(4,7);
      Serial.println(sp);
      setServo(servo1,sp.toInt());
    } else if (inputString.indexOf("Se2")==0)
    {
      Serial.println("Servo 2");
      sp = inputString.substring(4,7);
      setServo(servo2,sp.toInt());
    }
      else if (inputString.indexOf("Go")==0)
    {
        sp = inputString.substring(3,6);
        Serial.println("Go");
        Serial.println(sp.toInt());
        setMotor(sp.toInt(),sp.toInt(),true,true); 
    }
    else if (inputString.indexOf("Back")==0)
    {
        sp = inputString.substring(5,8);
        Serial.println(sp);
        Serial.println("Back");
        setMotor(sp.toInt(),sp.toInt(),false,false);
    }
    else if (inputString.indexOf("Stop")==0)
    {
       Serial.println("Stop");
       setMotor(0,0,true,true);
    }
    else if (inputString.indexOf("Right")==0)
    {
       sp = inputString.substring(6,9);
       Serial.println(sp);
       Serial.println("Right");
       setMotor(sp.toInt(),sp.toInt(),false,true);
    }
    else if (inputString.indexOf("Left")==0)
    {
      sp = inputString.substring(5,8);
      Serial.println(sp);
      Serial.println("Left");
      setMotor(sp.toInt(),sp.toInt(),true,false);
    }
    inputString = "";
    stringComplete = false;
    
  }
}

int returnDistance()
{
  int duration, distance;
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(1000);
  digitalWrite(trigPin,LOW);
  duration = pulseIn(echoPin,HIGH);
  distance = (duration/2 ) / 29.1;
  return distance;
}

void serialEvent() 
{
  while (Serial.available()) 
  {
    char inChar = (char)Serial.read(); 
    inputString += inChar;
    if (inChar == '\n') 
    {
      stringComplete = true;
      Serial.flush();
    } 
  }
}

void setServo(Servo servo, int pos)
{
  int curpos = servo.read();
    for(curpos;curpos<=pos;curpos++)
    {
      servo.write(curpos);
      delay(50);
       Serial.println(curpos);
    }

    for(curpos;curpos>=pos;curpos--)
    {
      servo.write(curpos);
      delay(50);
      Serial.println(curpos);
    }
}

void setMotor(int speedmotor1,int speedmotor2, boolean motor1, boolean motor2)
{
   analogWrite(ENB,speedmotor2);
   analogWrite(ENA,speedmotor1);
   digitalWrite(INA,motor1);
   digitalWrite(INB,!motor1);
   digitalWrite(INC,motor2);
   digitalWrite(IND,!motor2);
}
